#include "getnormal.h"
#include <pcl-1.8/pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl-1.8/pcl/point_types.h>
#include <iostream>

/**
 * @brief 使用3d搜索方法计算法线
 * 
 * @param cloud 
 * @return pcl::PointCloud<pcl::Normal>::Ptr 
 */
pcl::PointCloud<pcl::Normal>::Ptr findNormal(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud);
    //设置搜索方法
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setSearchMethod(tree);
    //设置输出数据集
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normal (new pcl::PointCloud<pcl::Normal>);
    //设置搜索半径 pcl中单位为m
    ne.setRadiusSearch(0.03);
    //计算
    ne.compute(*cloud_normal);
    return cloud_normal;
}

/**
 * @brief 使用积分图像计算法线
 * 
 * @param cloud 
 * @return pcl::PointCloud<pcl::Normal>::Ptr 
 */
pcl::PointCloud<pcl::Normal>::Ptr findNormal_integral(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
    ne.setMaxDepthChangeFactor(0.02f);
    ne.setNormalSmoothingSize(10.0f);
    ne.setInputCloud(cloud);
    ne.compute(*normals);
    return normals;
}